/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "coordinatemanager.h"
#include <qdebug.h>
#include <qfile.h>
#include <kdl/frames_io.hpp>
#include "md5check.h"
#include <QJsonDocument>
#include <QJsonArray>
#include "utcalculate.h"
#include "coordinatesolver.h"
#include "motionmessage.h"
#include "lasercameracalibrate.h"



CoordinateManager::CoordinateManager(RobotCommonParameter * roboParameterIn, MotionMessage *motionMessageIn)
{
    roboParameter=roboParameterIn;
    robotId=roboParameter->robotId;
    motionMessage=motionMessageIn;
    currentToolIndex=0;
    currentCameraIndex=0;
    currentUserIndex=0;
    currentUserIndex_old=0;

    if(-1==initCoordinate())
    {
        isInitialOk=false;
    }
    else
    {
        isInitialOk=true;
    }

    coordinateSolver=new CoordinateSolver(roboParameter->robotType,roboParameter->armControlAxisNumber,
                              roboParameter->robotId,   &roboParameter->jointAttributeVector,motionMessage
                              ,roboParameterIn->gravity);
    utCalculate=new UtCalculate(motionMessageIn, robotId,roboParameter->armControlAxisNumber, this);
//    laserCameraCalibrate=new LaserCameraCalibrate;


}

int CoordinateManager::getBestPositionFromAngle(double idealAngle, double initialPosition, double &bestPositionOut)
{
    return coordinateSolver->getBestPositionFromAngle(idealAngle,initialPosition,bestPositionOut);
}

int CoordinateManager::getIdealJointTorque(const QVector<double> &q_in, const QVector<double> &tmpQdot,
                                           const QVector<double> &tmpQdotdot, const QVector<double> &f_tool,
                                           QVector<double> &torqueOut)
{
    JntArray jntq_in(q_in.size());
    JntArray jnttmpQdot(q_in.size());
    JntArray jnttmpQdotdot(q_in.size());
    for(int i=0;i<q_in.size();i++)
    {
        if((roboParameter->jointAttributeVector)[i].jointType==0)//旋转关节
        {
            jntq_in(i) =q_in[i]*M_PI/180.0;//hualei todo ,will coredump.j PR[]
        }
        else//移动关节
        {
            jntq_in(i) =q_in[i]/ 1000.0;
        }
        jnttmpQdot(i)=tmpQdot[i];
        jnttmpQdotdot(i)=tmpQdotdot[i];
    }

//    jntf_tool. todo
    Vector f(0,0,0) ;
    Vector n(0,0,0) ;
    Wrench jntf_tool(f,n);

    JntArray jnttorqueOut(6);
    usingMutex.lock();
     int tmpKey=coordinateSolver->getIdealJointTorque(jntq_in,jnttmpQdot,jnttmpQdotdot,jntf_tool,jnttorqueOut);
     usingMutex.unlock();
     torqueOut.clear();
     for(int i=0;i<jnttorqueOut.rows();i++)
     {
         torqueOut.append(jnttorqueOut(i));
     }
     return tmpKey;
}

int CoordinateManager::setGravityVector(vector<double> gravityIn)
{
    return coordinateSolver->setGravityVector(gravityIn);
}

int CoordinateManager::setToolMass(int toolIndex,double mass, vector<double> center)
{
    if(3!=center.size())
    {
        qDebug()<<"error setToolMass,3!=center.size()"<<toolIndex;
        return -1;
    }
    if(toolIndex<0 || toolIndex>=toolCoordinateList.size())
    {
        qDebug()<<"error,toolIndex<0 || toolIndex>=toolCoordinateList.size()"<<toolIndex;
        return -2;
    }
    toolCoordinateList[toolIndex].mass=mass;
    toolCoordinateList[toolIndex].massCenter=QVector<double>::fromStdVector(center);
    if(1!=writeToolCoordinateList(toolCoordinateFile))
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5058, robotId);

    }
    else
    {
        addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5067, robotId,toolIndex);
    }
    if(currentToolIndex==toolIndex)
    {
        usingMutex.lock();
        coordinateSolver->setToolMass(mass,center);
        usingMutex.unlock();
    }
    return 1;
}

int CoordinateManager::setSegmentAttribute(int index, JointAttribute attributeIn)
{
    return coordinateSolver->setSegmentAttribute(index,attributeIn);
}

int CoordinateManager::getWeldPositionByCameraTransfer(const PointPro tcpPosition, double x, double y,
                                                       double z, PointPro &pointOut)
{
    if(0==cameratoolCoordinateList.size())
    {
        return 0;
    }
    //坐标锁问题
    PointPro tmpPosition;
    pointCoordinateToCartesian(tcpPosition, tmpPosition);
    Frame frame_tcp_in_world = Frame(createEulerMatrix(tmpPosition.positionValue[3]*M_PI/180.0,
                    tmpPosition.positionValue[4]*M_PI/180.0,
                    tmpPosition.positionValue[5]*M_PI/180.0),
                    Vector (tmpPosition.positionValue[0]/1000.0,
                        tmpPosition.positionValue[1]/1000.0,
                        tmpPosition.positionValue[2]/1000.0));

    Frame frame_weld_in_camera = Frame(Vector (x/1000.0,y/1000.0,z/1000.0));

    usingMutex.lock();
    KDL::Frame frame_weld_in_world=frame_tcp_in_world*cameratoolCoordinateList[0].forwardTcpMatrix*frame_weld_in_camera;
    usingMutex.unlock();

    pointOut.positionValue.resize(6);
    pointOut.positionValue[0]=frame_weld_in_world.p[0]*1000.0;
    pointOut.positionValue[1]=frame_weld_in_world.p[1]*1000.0;
    pointOut.positionValue[2]=frame_weld_in_world.p[2]*1000.0;
    pointOut.positionValue[3]=0;
    pointOut.positionValue[4]=0;
    pointOut.positionValue[5]=0;
    return 1;
}

Rotation CoordinateManager::createEulerMatrix(double psi, double sita, double fai)
{
   return coordinateSolver->createEulerMatrix(psi,sita,fai);
}

void CoordinateManager::extractEulerMatrixAngle(const Rotation &rotMat, double &psi, double &sita, double &fai)
{
    return coordinateSolver->extractEulerMatrixAngle(rotMat,psi,sita,fai);
}


int CoordinateManager::calculateLaserCameraFrame(int indexIn, std::vector<PointPro> tcpPointList,
                              std::vector<PointPro> cameraPointList, LaserCameraCalibrateResult &resultOut)
{

    LaserCameraCalibrate laserCameraCalibrate;
    if(1==laserCameraCalibrate.calculateLaserCameraFrame(tcpPointList,cameraPointList,
                                                         resultOut.cameraTransfer,resultOut.maxError))
    {


        ToolCoordinateInfo_frame tmpCoordinate;
        tmpCoordinate.index=0;
        tmpCoordinate.error=resultOut.maxError;
        tmpCoordinate.method=0;
        for(int i=0;i<tcpPointList.size();i++)
        {
            tmpCoordinate.pointList.append(tcpPointList[i]);
        }
        for(int i=0;i<cameraPointList.size();i++)
        {
            tmpCoordinate.pointList.append(cameraPointList[i]);
        }
        for(int i=0;i<resultOut.cameraTransfer.size();i++)
        {
            tmpCoordinate.xyzwpr.append(resultOut.cameraTransfer[i]);
        }

        tmpCoordinate.forwardTcpMatrix.M=coordinateSolver->createEulerMatrix(
                    resultOut.cameraTransfer[3]/180.0*M_PI,
                    resultOut.cameraTransfer[4]/180.0*M_PI,
                    resultOut.cameraTransfer[5]/180.0*M_PI);

        tmpCoordinate.forwardTcpMatrix.p[0]=resultOut.cameraTransfer[0]/1000.0;
        tmpCoordinate.forwardTcpMatrix.p[1]=resultOut.cameraTransfer[1]/1000.0;
        tmpCoordinate.forwardTcpMatrix.p[2]=resultOut.cameraTransfer[2]/1000.0;
        tmpCoordinate.inverseTcpMatrix=tmpCoordinate.forwardTcpMatrix.Inverse();

        usingMutex.lock();
        cameratoolCoordinateList[0]=tmpCoordinate;
        usingMutex.unlock();
        writeCameraCoordinateList(cameraCoordinateFile);
        resultOut.isSuccess=1;
        return 1;
    }
    resultOut.isSuccess=0;
    return -1;
}

int CoordinateManager::getLaserCameraFrameInfo(LaserCameraFrameInfo &infoOut)
{
    usingMutex.lock();
    if(10==cameratoolCoordinateList[0].pointList.size())
    {
        for(int i=0;i<5;i++)
        {
            infoOut.tcpPointList.push_back(cameratoolCoordinateList[0].pointList[i]);
        }
    //    PointPro tmpPoint;
    //    tmpPoint.positionValue.resize(6);
    //    infoOut.cameraPointList.push_back(tmpPoint);
        for(int i=0;i<5;i++)
        {
            infoOut.cameraPointList.push_back(cameratoolCoordinateList[0].pointList[i+5]);
        }

    }

    infoOut.cameraTransfer=cameratoolCoordinateList[0].xyzwpr.toStdVector();
    infoOut.maxError=cameratoolCoordinateList[0].error;
    usingMutex.unlock();
    return 1;
}

int CoordinateManager::pointCoordinateToCartesian_cameraPoint(const PointPro jointPointIn,
                                       const PointPro cameraPointIn, PointPro &worldPointOut)
{
    if(jointPointIn.jointFlag==JOINT_FLAG_CARTESIAN)
    {
        qDebug()<<"error, pointCoordinateToCartesian_cameraPoint , pointIn.jointFlag==JOINT_FLAG_CARTESIAN";
        return -1;
    }
    if(1!=coordinateSolver->pointCoordinateToCartesian(jointPointIn,worldPointOut))
    {
        return -1;
    }
    pointAddCameraCoordinate(cameraPointIn,worldPointOut);
    worldPointOut.ufValue = currentUserIndex;
    worldPointOut.utValue = currentToolIndex;
    return 1;
}


int CoordinateManager::initCoordinate()
{
    userCoordinateList.resize(COORDINATE_MAX_SIZE);
    toolCoordinateList.resize(COORDINATE_MAX_SIZE);
    cameratoolCoordinateList.resize(COORDINATE_MAX_SIZE);
    Frame originFrame = Frame::Identity();
    QVector<double> originQvector(6, 0.0);
    for(int i=0; i<COORDINATE_MAX_SIZE; i++)
    {
        userCoordinateList[i].index = i;
        userCoordinateList[i].method = ENUM_USER_XYZWPR;
        userCoordinateList[i].forwardTcpMatrix = originFrame;
        userCoordinateList[i].inverseTcpMatrix = originFrame;
        userCoordinateList[i].xyzwpr=originQvector;
        userCoordinateList[i].pointList.resize(6);
//        userCoordinateList[i].mass=0;
//        userCoordinateList[i].massCenter.resize(3);

        toolCoordinateList[i].index = i;
        toolCoordinateList[i].error = 0.0;
        toolCoordinateList[i].method = ENUM_TOOL_XYZWPR;
        toolCoordinateList[i].forwardTcpMatrix = originFrame;
        toolCoordinateList[i].inverseTcpMatrix = originFrame;
        toolCoordinateList[i].xyzwpr=originQvector;
        toolCoordinateList[i].pointList.resize(6);
        toolCoordinateList[i].mass=0;
        toolCoordinateList[i].massCenter.resize(3);

        cameratoolCoordinateList[i].index = i;
        cameratoolCoordinateList[i].error = 0.0;
        cameratoolCoordinateList[i].method = ENUM_TOOL_XYZWPR;
        cameratoolCoordinateList[i].forwardTcpMatrix = originFrame;
        cameratoolCoordinateList[i].inverseTcpMatrix = originFrame;
        cameratoolCoordinateList[i].xyzwpr=originQvector;
        cameratoolCoordinateList[i].pointList.resize(6);
        cameratoolCoordinateList[i].mass=0;
//        toolCoordinateList[i].massCenter.resize(3);
    }

    QString tmpFilePath=D_TCRCONTROLLER_DATA_PATH;
    toolCoordinateFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/dat/toolCoordinateInfo";
    userCoordinateFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/dat/userCoordinateInfo";
    cameraCoordinateFile = tmpFilePath+ROBOT_COMMON_NAME+QString::number( robotId )+"/dat/cameraCoordinateInfo";

    currentToolIndex=0;
    currentUserIndex=0;

    if(0)//可以用于初始化生成配置文件
    {
            writeToolCoordinateList(toolCoordinateFile);
            writeUserCoordinateList(userCoordinateFile);
            writeCameraCoordinateList(cameraCoordinateFile);
    }


    if(1!=readToolCoordinateList(toolCoordinateFile) )
    {
        qDebug()<<"error,readToolCoordinateList";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5056, robotId);
       return -1;
    }
    if( 1!=readUserCoordinateList(userCoordinateFile))
    {
        qDebug()<<"error,readUserCoordinateList";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5055, robotId);
       return -2;
    }
    if(1!=readCameraCoordinateList(cameraCoordinateFile) )
    {
        qDebug()<<"error,cameraCoordinateFile";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5066, robotId);
       return -1;
    }
    return 1;
}

bool CoordinateManager::getInitialStatus()
{
    return isInitialOk;
}

QVector<ToolCoordinateInfo> CoordinateManager::getToolCoordinateList()
{
    QVector<ToolCoordinateInfo> tmpList;
    ToolCoordinateInfo tmpInfo;
    for(int i=0;i<toolCoordinateList.size();i++)
    {
        tmpInfo.error=toolCoordinateList[i].error;
        tmpInfo.index=toolCoordinateList[i].index;
        tmpInfo.method=toolCoordinateList[i].method;
        tmpInfo.pointList=toolCoordinateList[i].pointList;
        tmpInfo.xyzwpr=toolCoordinateList[i].xyzwpr;
        tmpInfo.mass=toolCoordinateList[i].mass;
        tmpInfo.massCenter=toolCoordinateList[i].massCenter;
        tmpInfo.name=toolCoordinateList[i].name;
        tmpList.append(tmpInfo);
    }

    return tmpList;
}

QVector<UserCoordinateInfo> CoordinateManager::getUserCoordinateList()
{
    QVector<UserCoordinateInfo> tmpList;
    UserCoordinateInfo tmpInfo;
    for(int i=0;i<userCoordinateList.size();i++)
    {
        tmpInfo.index=userCoordinateList[i].index;
        tmpInfo.method=userCoordinateList[i].method;
        tmpInfo.pointList=userCoordinateList[i].pointList;
        tmpInfo.xyzwpr=userCoordinateList[i].xyzwpr;
        tmpInfo.name=userCoordinateList[i].name;
        tmpList.append(tmpInfo);
    }

    return tmpList;
}

int CoordinateManager::getCurrentToolForwardMatrix(Frame &frameReturn)
{
    frameReturn=toolCoordinateList[currentToolIndex].forwardTcpMatrix;
    return 1;
}

int CoordinateManager::getCurrentCameraForwardMatrix(Frame &frameReturn)
{
    frameReturn=cameratoolCoordinateList[currentCameraIndex].forwardTcpMatrix;
    return 1;
}

int CoordinateManager::getCurrentToolInverseMatrix(Frame &frameReturn)
{
    frameReturn=toolCoordinateList[currentToolIndex].inverseTcpMatrix;
    return 1;
}

int CoordinateManager::getCurrentUserForwardMatrix(Frame &frameReturn)
{
    frameReturn=userCoordinateList[currentUserIndex].forwardTcpMatrix;
//    frameReturn=userCoordinateList[currentUserIndex].inverseTcpMatrix;//需要反下
    return 1;
}

int CoordinateManager::getCurrentUserInverseMatrix(Frame &frameReturn)
{
        frameReturn=userCoordinateList[currentUserIndex].inverseTcpMatrix;
//    frameReturn=userCoordinateList[currentUserIndex].forwardTcpMatrix;//需要反下

    return 1;
}

int CoordinateManager::getCurrentToolIndex()
{
    return currentToolIndex;
}

int CoordinateManager::getCurrentCameraIndex()
{
    return currentCameraIndex;
}


int CoordinateManager::getCurrentUserIndex()
{
    return currentUserIndex;
}

int CoordinateManager::activateCameraCoordinate(int index)
{
    if(index>COORDINATE_MAX_SIZE-1 ||  index <0)
    {
        qDebug()<<"activateCameraCoordinate error,index>COORDINATE_MAX_SIZE-1 ||  index <0 ";
        return -1;
    }
    currentCameraIndex=index;
    return 1;
}

int CoordinateManager::activateToolCoordinate(int index)
{
    if(index>COORDINATE_MAX_SIZE-1 ||  index <0)
    {
        qDebug()<<"activateToolCoordinate error,index>COORDINATE_MAX_SIZE-1 ||  index <0 ";
        return -1;
    }
    currentToolIndex=index;
    usingMutex.lock();
    coordinateSolver->setToolMass(toolCoordinateList[index].mass
                                  ,toolCoordinateList[index].massCenter.toStdVector());
    usingMutex.unlock();
    return 1;
}

int CoordinateManager::activateUserCoordinate(int index)
{
    if(index>COORDINATE_MAX_SIZE-1 ||  index <0)
    {
        qDebug()<<"activateUserCoordinate error,index>COORDINATE_MAX_SIZE-1 ||  index <0 ";
        return -1;
    }
    currentUserIndex_old=currentUserIndex;
    currentUserIndex=index;
    return 1;
}

int CoordinateManager::restoreUserCoordinate()
{
    currentUserIndex=currentUserIndex_old;
    return 1;
}

int CoordinateManager::setToolCoordinate(ToolCoordinateInfo& toolCoordinate, bool isWriteFile)
{

    ToolCoordinateInfo_frame tmpInfo;
    tmpInfo.error=toolCoordinate.error;
    tmpInfo.index=toolCoordinate.index;
    tmpInfo.method=toolCoordinate.method;
    tmpInfo.pointList=toolCoordinate.pointList;
    tmpInfo.xyzwpr=toolCoordinate.xyzwpr;
    tmpInfo.name=toolCoordinate.name;

    if(1==utCalculate->setToolCoordinate(tmpInfo,toolCoordinateList))
    {
        switch(toolCoordinate.method)
        {
        case ENUM_TOOL_6POINTS:
        {
            tmpInfo.pointList.resize(3);
            break;
        }
        case ENUM_TOOL_3POINTS:
        {
            tmpInfo.pointList.resize(3);
            break;
        }
        case ENUM_TOOL_20POINTS_ZEROCAL:
        case ENUM_TOOL_20POINTS:
        {
            tmpInfo.pointList.resize(20);
            break;
        }
        case ENUM_TOOL_23POINTS:
        {
            tmpInfo.pointList.resize(20);
            break;
        }
        case ENUM_TOOL_XYZWPR://直接法
        {
            tmpInfo.pointList.resize(0);
            break;
        }
        default://不支持工具坐标系设定方法
        {
            tmpInfo.pointList.resize(0);
        }
        }

        //求最大误差
        activateToolCoordinate(tmpInfo.index);
        double maxError;
        calculateMaxError(tmpInfo.pointList.toStdVector(),maxError );
        tmpInfo.error=maxError;

        toolCoordinate.error=maxError;
        toolCoordinate.xyzwpr=toolCoordinateList[toolCoordinate.index].xyzwpr;
        toolCoordinateList[toolCoordinate.index].error=maxError;

        if(isWriteFile)
        {
            if(1!=writeToolCoordinateList(toolCoordinateFile))
            {
                addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5058, robotId);

            }
            else
            {
                addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5060, robotId,tmpInfo.index);
            }
        }
    }
    else
    {
        switch(toolCoordinate.method)
        {
        case ENUM_TOOL_20POINTS_ZEROCAL:
        case ENUM_TOOL_20POINTS:
        case ENUM_TOOL_23POINTS:
        case ENUM_TOOL_6POINTS:
        {
            if(isWriteFile)
            {
                if(1!=writeToolCoordinateList(toolCoordinateFile))
                {
                    addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5058, robotId);

                }
                else
                {
                    addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5060, robotId,tmpInfo.index);
                }
            }
            break;
        }
        }




        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5062, robotId,tmpInfo.index);
        return -1;
    }


    return 1;
}

int CoordinateManager::setToolCoordinate_20pOfZero(ToolCoordinateInfo &toolCoordinate,
                                                   vector<double> &zeroOffsetOut)
{
    addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5068, robotId);
    int maxInitiationCount=10;
    ToolCoordinateInfo toolCoordinateTest=toolCoordinate;
    double axisOffsetOut_0,axisOffsetOut_1,axisOffsetOut_2,axisOffsetOut_3,axisOffsetOut_4,axisOffsetOut_5;
    double abs[20];
    for(int i=0;i<20;i++)
    {
        abs[i]=0;
    }

    double deltaMove=0.02;
    double range=3;
    int m;
    for(int m=0;m<maxInitiationCount;m++)
    {

        //只校准关节轴，直线轴不处理。第一个关节轴和最后一个关节轴也不处理。
        switch (roboParameter->robotType) {
        case ENUM_SCARA:
            zeroOffsetOut.resize(4);
//            if(0==axisId || 2==axisId || 3==axisId)
//            {
//                isNeedCalculate=false;
//            }
            zeroOffsetOut.resize(4);
            calculateOneAxisZeroOffset(1,deltaMove,range,toolCoordinateTest,axisOffsetOut_1);//1.9,-1.7,1.8
            abs[1]+=axisOffsetOut_1;
            for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            {
                toolCoordinateTest.pointList[k].positionValue[1]+=axisOffsetOut_1;
            }
            break;
        case ENUM_6AXIS_CHAIN:
            zeroOffsetOut.resize(6);
            //        calculateOneAxisZeroOffset(0,toolCoordinateTest,axisOffsetOut_0);
            //        abs_0+=axisOffsetOut_0;
            //        for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            //        {
            //            toolCoordinateTest.pointList[k].positionValue[0]+=axisOffsetOut_0;
            //        }

            calculateOneAxisZeroOffset(1,deltaMove,range,toolCoordinateTest,axisOffsetOut_1);//1.9,-1.7,1.8
            abs[1]+=axisOffsetOut_1;
            for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            {
                toolCoordinateTest.pointList[k].positionValue[1]+=axisOffsetOut_1;
            }
            calculateOneAxisZeroOffset(2,deltaMove,range,toolCoordinateTest,axisOffsetOut_2);//0.8, -0.4,0
            abs[2]+=axisOffsetOut_2;
            for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            {
                toolCoordinateTest.pointList[k].positionValue[2]+=axisOffsetOut_2;
            }
            calculateOneAxisZeroOffset(3,deltaMove,range,toolCoordinateTest,axisOffsetOut_3);//0.1, -0.1,0
            abs[3]+=axisOffsetOut_3;
            for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            {
                toolCoordinateTest.pointList[k].positionValue[3]+=axisOffsetOut_3;
            }
            calculateOneAxisZeroOffset(4,deltaMove,range,toolCoordinateTest,axisOffsetOut_4);
            abs[4]+=axisOffsetOut_4;

            for(int k=0;k<toolCoordinateTest.pointList.size();k++)
            {
                toolCoordinateTest.pointList[k].positionValue[4]+=axisOffsetOut_4;
            }
            break;
        case ENUM_DESCART_WRAC_ROBOT:
        case ENUM_DESCART_WRACTEST_ROBOT:
        case ENUM_DESCART_WRIST_ROBOT:
//            if(0==axisId || 1==axisId || 2==axisId || 3==axisId || 5==axisId)
//            {
//                isNeedCalculate=false;
//            }
            break;
        default:
        {
            qDebug()<<"error,,,ZeroCalculate::calculateZeroOffset_allAxis unsupport robotType";
            return -1;
        }
        }




//        calculateOneAxisZeroOffset(5,toolCoordinateTest,axisOffsetOut_5);
//        abs_5+=axisOffsetOut_5;

//        for(int k=0;k<toolCoordinateTest.pointList.size();k++)
//        {
//            toolCoordinateTest.pointList[k].positionValue[4]+=axisOffsetOut_5;
//        }

        qDebug()<<"m"<<m<<axisOffsetOut_0 <<axisOffsetOut_1<<axisOffsetOut_2
               <<axisOffsetOut_3<<axisOffsetOut_4<<axisOffsetOut_5;
        qDebug()<<"---------------------------------------------------------------------------";
        if(axisOffsetOut_1<deltaMove && axisOffsetOut_2<deltaMove && axisOffsetOut_3<deltaMove &&
                axisOffsetOut_4<deltaMove)
        {
            qDebug()<<"符合收敛条件，提前终止,运算次数"<<m;
            break;
        }
    }

    if(m==maxInitiationCount-1)
    {
        qDebug()<<"迭代次数耗尽,运算次数"<<m;
    }

    for(int i=0;i<zeroOffsetOut.size();i++)
    {
        zeroOffsetOut[i]=abs[i];
    }

    qDebug()<<"----------zeroOffsetOut"<<QVector<double>::fromStdVector( zeroOffsetOut);

    toolCoordinate=toolCoordinateTest;
    setToolCoordinate(toolCoordinate,true);
    QString tmpMsg=",推算新原点:";
    tmpMsg+=QString::number(-zeroOffsetOut[0])+","+QString::number(-zeroOffsetOut[1])+","+QString::number(-zeroOffsetOut[2])+","
            +QString::number(-zeroOffsetOut[3])+","+QString::number(-zeroOffsetOut[4])+","+QString::number(-zeroOffsetOut[5]);
    addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5069, robotId,0,0,0,0,tmpMsg);
    return 1;
}

int CoordinateManager::calculateOneAxisZeroOffset(int axisId,double deltaMove,double range, ToolCoordinateInfo &toolCoordinate
                                                  ,double &axisOffsetOut)
{


    QVector<double> errorList;

    int pcount=range/deltaMove;
    ToolCoordinateInfo testCoordinate=toolCoordinate;
    for(int i=0;i<pcount;i++)
    {
        for(int k=0;k<testCoordinate.pointList.size();k++)
        {
            testCoordinate.pointList[k].positionValue[axisId]=toolCoordinate.pointList[k].positionValue[axisId]
                -deltaMove*pcount/2+deltaMove*i;
        }
        setToolCoordinate(testCoordinate,false);
        errorList.append(testCoordinate.error);
    }
    double minError=10000000;
    int minIndex;
    for( int m=0;m<errorList.size();m++)
    {
        if(minError>errorList[m])
        {
            minError=errorList[m];
            minIndex=m;
        }
    }
    axisOffsetOut=-deltaMove*pcount/2+deltaMove*minIndex;
    qDebug()<<"axisId"<<axisId<<"errorList"<<errorList<<"m"<<minIndex<<"axisOffsetOut"<<axisOffsetOut;
    return 1;
}

int CoordinateManager::setUserCoordinate(UserCoordinateInfo& userCoordinate)
{

    UserCoordinateInfo_frame tmpInfo;
    tmpInfo.index=userCoordinate.index;
    tmpInfo.method=userCoordinate.method;
    tmpInfo.pointList=userCoordinate.pointList;
    tmpInfo.xyzwpr=userCoordinate.xyzwpr;
    tmpInfo.name=userCoordinate.name;

    if(1==utCalculate->setUserCoordinate(tmpInfo,toolCoordinateList,userCoordinateList))
    {
        userCoordinate.xyzwpr=userCoordinateList[userCoordinate.index].xyzwpr;
        if(1!=writeUserCoordinateList(userCoordinateFile))
        {
            addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5057, robotId);

        }
        else
        {
            addMsg(ENUM_MSG_REMIND, "McControllerInterpreter", "CoordinateManager", 5059, robotId,tmpInfo.index);
        }
    }
    else
    {
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5061, robotId,tmpInfo.index);
        return -1;
    }

    return 1;
}

int CoordinateManager::moveUserCoordinate(int coordinateIndexIn, double move_x, double move_y, double move_z)
{
     if(coordinateIndexIn<0 || coordinateIndexIn>=userCoordinateList.size())
     {
         return -1;
     }

    userCoordinateList[coordinateIndexIn].forwardTcpMatrix.p[0]-=move_x;
    userCoordinateList[coordinateIndexIn].forwardTcpMatrix.p[1]-=move_y;
    userCoordinateList[coordinateIndexIn].forwardTcpMatrix.p[2]-=move_z;
    qDebug()<<" coordinateIndexIn"<<coordinateIndexIn<<" p[0]"<<userCoordinateList[coordinateIndexIn].forwardTcpMatrix.p[0];
    userCoordinateList[coordinateIndexIn].inverseTcpMatrix=userCoordinateList[coordinateIndexIn].forwardTcpMatrix.Inverse();
    return 1;
}

int CoordinateManager::copyUserCoordinate(int sourceCoordinateIndexIn, int targetCoordinateIndexIn,
                                          double offset_x,double offset_y,double offset_z)
{
    if(sourceCoordinateIndexIn<0 || sourceCoordinateIndexIn>=userCoordinateList.size())
    {
        return -1;
    }
    if(targetCoordinateIndexIn<0 || targetCoordinateIndexIn>=userCoordinateList.size())
    {
        return -1;
    }
//    userCoordinateList[targetCoordinateIndexIn]=userCoordinateList[sourceCoordinateIndexIn];
//    userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[0]=
//            userCoordinateList[sourceCoordinateIndexIn].forwardTcpMatrix.p[0]+offset_x;
//    userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[1]=
//            userCoordinateList[sourceCoordinateIndexIn].forwardTcpMatrix.p[1]+offset_y;
//    userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[2]=
//            userCoordinateList[sourceCoordinateIndexIn].forwardTcpMatrix.p[2]+offset_z;

    userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.M=userCoordinateList[sourceCoordinateIndexIn].inverseTcpMatrix.M;

    userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.p[0]=
            userCoordinateList[sourceCoordinateIndexIn].inverseTcpMatrix.p[0]-offset_x;
    userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.p[1]=
            userCoordinateList[sourceCoordinateIndexIn].inverseTcpMatrix.p[1]-offset_y;
    userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.p[2]=
            userCoordinateList[sourceCoordinateIndexIn].inverseTcpMatrix.p[2]-offset_z;

//    qDebug()<<" targetCoordinateIndexIn"<<targetCoordinateIndexIn<<" p[0]"
//    <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[0]
//           <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[1]
//          <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.p[2];

//         <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[0]
//           <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[1]
//             <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[2]
//               <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[3]
//                 <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[4]
//                   <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[5]
//                     <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[6]
//                       <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[7]
//                         <<userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix.M.data[8];
    userCoordinateList[targetCoordinateIndexIn].forwardTcpMatrix=
            userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.Inverse();
    if(0)
    {
        qDebug()<<"userCoordinateList"<<userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.p[0]
               <<userCoordinateList[targetCoordinateIndexIn].inverseTcpMatrix.p[1];
    }

    return 1;

}

int CoordinateManager::equalUserCoordinate(int sourceCoordinateIndex, int targetCoordinateIndex)
{
    if(sourceCoordinateIndex<0 || sourceCoordinateIndex>=userCoordinateList.size())
    {
        return -1;
    }
    if(targetCoordinateIndex<0 || targetCoordinateIndex>=userCoordinateList.size())
    {
        return -1;
    }
    userCoordinateList[targetCoordinateIndex].forwardTcpMatrix=userCoordinateList[sourceCoordinateIndex].forwardTcpMatrix;
    userCoordinateList[targetCoordinateIndex].inverseTcpMatrix=userCoordinateList[sourceCoordinateIndex].inverseTcpMatrix;
    return 1;
}



int CoordinateManager::pointCoordinateToJoint(const PointPro pointIn, vector<double> positionInitialList
                                              , PointPro &jointPointOut, bool isErrorNotice)
{
    if(pointIn.jointFlag==JOINT_FLAG_JOINT)
    {
//        qDebug()<<"pointCoordinateToJoint warnning pointIn.jointFlag==JOINT_FLAG_JOINT";
        jointPointOut=pointIn;
        return 1;
    }

    jointPointOut=pointIn;
    if(ENUM_DESCART_WRACTEST_ROBOT!=roboParameter->robotType)
    {
        pointDeleteCoordinate(jointPointOut);
    }
    return coordinateSolver->pointCoordinateToJoint(jointPointOut,positionInitialList,jointPointOut
                                                    ,isErrorNotice);

}

int CoordinateManager::kinematicsInverse(std::vector<double> posIn, std::vector<double> initialJointIn,
                                         std::vector<double> &jointOut, bool isErrorNotice)
{
    PointPro tmpPoint;
    tmpPoint.jointFlag=JOINT_FLAG_CARTESIAN;
    tmpPoint.positionValue=posIn;
    tmpPoint.ufValue=currentUserIndex;
    tmpPoint.utValue=currentToolIndex;
    if(ENUM_DESCART_WRACTEST_ROBOT!=roboParameter->robotType)
    {
        if(1!=pointDeleteCoordinate(tmpPoint))
        {
            return -1;
        }
    }

    PointPro tmpjointPointOut;
    int tmpKey= coordinateSolver->pointCoordinateToJoint(tmpPoint,initialJointIn,tmpjointPointOut
                                                    ,isErrorNotice);
    jointOut=tmpjointPointOut.positionValue;
    return tmpKey;
}



int CoordinateManager::pointCoordinateToCartesian(const PointPro pointIn, PointPro &pointOut)
{

    if(pointIn.jointFlag==JOINT_FLAG_CARTESIAN)
    {
//        qDebug()<<"pointCoordinateToCartesian warnning, pointIn.jointFlag==JOINT_FLAG_CARTESIAN";
        pointOut=pointIn;
        return 1;
    }

    if(1!=coordinateSolver->pointCoordinateToCartesian(pointIn,pointOut))
    {
        return -1;
    }
    if(ENUM_DESCART_WRACTEST_ROBOT!=roboParameter->robotType)
    {
        pointAddCoordinate(pointOut);
    }

    pointOut.ufValue = currentUserIndex;
    pointOut.utValue = currentToolIndex;
    return 1;
}

Frame CoordinateManager::userFrameToWorldFrame(const Frame &userFrameIn)
{
    Frame tmpFrameOut;
    Frame toolInvertFrame;
    Frame userForwardFrame;

    //0号预留为单元矩阵
    if(0==getCurrentToolIndex())
    {
        toolInvertFrame = Frame::Identity();
    }
    else
    {
        getCurrentToolInverseMatrix(toolInvertFrame);
    }
    if(0==getCurrentUserIndex())
    {
        userForwardFrame = Frame::Identity();
    }
    else
    {
        getCurrentUserForwardMatrix(userForwardFrame);
    }

//    pointFrame=pointFrame*toolInvertFrame*userInvertFrame;
//    pointFrame=toolInvertFrame*pointFrame*userForwardFrame;

    tmpFrameOut=userForwardFrame*userFrameIn*toolInvertFrame;
    return tmpFrameOut;
}

Frame CoordinateManager::pointProToFrame_noUT(const PointPro &pointPro)
{
    Frame tmpFrame;
    int tmpKey=coordinateSolver->pointProToFrame_noUT(pointPro,tmpFrame);
    if(1!=tmpKey)
    {
        qDebug()<<"error,,,,,CoordinateManager::pointProToFrame_noUT"<<tmpKey;
    }
    return tmpFrame;
}


int CoordinateManager::calculateMaxError(std::vector<PointPro> pointList,  double &maxErrorOut)
{
    qDebug()<<"calculateMaxError size"<<pointList.size();
    std::vector<PointPro> pointListOut=pointList;

    //求坐标点
    for(int i=0;i<pointList.size();i++)
    {
        pointCoordinateToCartesian(pointList[i],pointListOut[i]);
    }
    //比较最大值
    maxErrorOut=0;
    for(int i=0;i<pointListOut.size();i++)
    {
        for(int j=i+1;j<pointListOut.size();j++)
        {
            double tmpCurrentError=sqrt((pointListOut[i].positionValue[0]-pointListOut[j].positionValue[0])
                    *(pointListOut[i].positionValue[0]-pointListOut[j].positionValue[0])
                    +(pointListOut[i].positionValue[1]-pointListOut[j].positionValue[1])
                                        *(pointListOut[i].positionValue[1]-pointListOut[j].positionValue[1])
                    +(pointListOut[i].positionValue[2]-pointListOut[j].positionValue[2])
                                        *(pointListOut[i].positionValue[2]-pointListOut[j].positionValue[2]));
            if(tmpCurrentError>maxErrorOut)
            {
                maxErrorOut=tmpCurrentError;
            }
//            qDebug()<<i<<j<<"error distance "<<tmpCurrentError;

        }
    }
    return 1;

}

int CoordinateManager::pointDeleteCoordinate(PointPro& decartPoint)
{
    if(decartPoint.jointFlag==JOINT_FLAG_JOINT)
    {
        qDebug()<<"pointDeleteCoordinate error, pointAddCoordinate。";
        return -1;
    }
    Frame pointFrame;
    if(1!=coordinateSolver->pointProToFrame_noUT(decartPoint,pointFrame))
    {
        return -2;
    }
    pointFrame=userFrameToWorldFrame(pointFrame);


    PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
    decartPoint.positionValue=tmpPoint.positionValue;
    if(currentUserIndex!=decartPoint.ufValue)
    {
        qDebug()<<"pointDeleteCoordinate　点位信息里面存储的用户坐标号与但前用户坐标号不一致";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5051, robotId,decartPoint.ufValue,currentUserIndex);
    }
    if(currentToolIndex!=decartPoint.utValue)
    {
        qDebug()<<"pointDeleteCoordinate　点位信息里面存储的工具坐标号与但前工具坐标号不一致";
        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5052, robotId,decartPoint.utValue,currentToolIndex);
    }
    return 1;
}

int CoordinateManager::pointAddCoordinate(PointPro& decartPoint)
{
    if(decartPoint.jointFlag==JOINT_FLAG_JOINT)
    {
        qDebug()<<"pointAddCoordinate error, pointAddCoordinate。";
        return -1;
    }

//    Frame pointFrame=coordinateSolver->pointProToFrame(decartPoint);
    Frame pointFrame;
    if(1!=coordinateSolver->pointProToFrame_noUT(decartPoint,pointFrame))
    {
        return -2;
    }
    Frame toolForwardFrame;
    Frame userInverseFrame;
    //0号预留为单元矩阵
    if(0==getCurrentToolIndex())
    {
        toolForwardFrame = Frame::Identity();
    }
    else
    {
        getCurrentToolForwardMatrix(toolForwardFrame);
    }
    if(0==getCurrentUserIndex())
    {
        userInverseFrame = Frame::Identity();
    }
    else
    {
        getCurrentUserInverseMatrix(userInverseFrame);
    }
//    pointFrame=pointFrame*userForwardFrame*toolForwardFrame;
//    pointFrame=toolForwardFrame*pointFrame*userInverseFrame;
    pointFrame=userInverseFrame*pointFrame*toolForwardFrame;
    PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
    decartPoint.positionValue=tmpPoint.positionValue;
    return 1;
}

int CoordinateManager::pointAddCameraCoordinate(const PointPro cameraPointIn, PointPro &decartPoint)
{
    if(decartPoint.jointFlag==JOINT_FLAG_JOINT)
    {
        qDebug()<<"pointAddCameraCoordinate error, pointAddCoordinate。";
        return -1;
    }

    if(6!=cameraPointIn.positionValue.size())
    {
        qDebug()<<"pointAddCameraCoordinate error, 6!=cameraPointIn.positionValue.size()";
        return -2;
    }

//    Frame pointFrame=coordinateSolver->pointProToFrame(decartPoint);
    Frame pointFrame;
    if(1!=coordinateSolver->pointProToFrame_noUT(decartPoint,pointFrame))
    {
        return -3;
    }
    Frame toolForwardFrame;
    Frame userInverseFrame;

    getCurrentCameraForwardMatrix(toolForwardFrame);

    if(0==getCurrentUserIndex())
    {
        userInverseFrame = Frame::Identity();
    }
    else
    {
        getCurrentUserInverseMatrix(userInverseFrame);
    }


    Frame cameraOffsetForwardFrame(Vector(cameraPointIn.positionValue[0],
                        cameraPointIn.positionValue[1],
                        cameraPointIn.positionValue[2]));

    pointFrame=userInverseFrame*pointFrame*toolForwardFrame*cameraOffsetForwardFrame;
    PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(pointFrame);
    decartPoint.positionValue=tmpPoint.positionValue;
    return 1;
}

int CoordinateManager::pointProToFrame_noUT(const PointPro& pointPro,Frame &frameOut)
{
    return coordinateSolver->pointProToFrame_noUT(pointPro,frameOut);
}

PointPro CoordinateManager::frameToPointPro_CART(const Frame& f)
{
    PointPro tmpPoint=coordinateSolver->frameToPointPro_CART(f);
    tmpPoint.ufValue=getCurrentUserIndex();
    tmpPoint.utValue=getCurrentToolIndex();
    return tmpPoint;
}

int CoordinateManager::userFrameToPointPro_JOINT(const Frame& frameIn, PointPro& pointOut, bool &isSinguratyOut,
              string configString, vector<double> positionInitialList, bool isErrorNotice)
{
    isSinguratyOut=false;
    pointOut=coordinateSolver->frameToPointPro_CART(frameIn);
    pointOut.ufValue=currentUserIndex;
    pointOut.utValue=currentToolIndex;
    pointDeleteCoordinate(pointOut);
    Frame tmpFrame;
    if(1!=pointProToFrame_noUT(pointOut,tmpFrame))
    {
        return -1;
    }
    return worldFrameToPointPro_JOINT(tmpFrame, pointOut,isSinguratyOut,configString,positionInitialList,isErrorNotice);
//    if(1!=coordinateSolver->frameToPointPro_JOINT(tmpFrame, pointOut,configString,positionInitialList))
//    {
//        return -1;
//    }
//    return 1;
}

int CoordinateManager::worldFrameToPointPro_JOINT(const Frame &frameIn, PointPro &pointOut,
                                                  bool &isSinguratyOut,
                     string configString, vector<double> positionInitialList, bool isErrorNotice)
{
    if(1!=coordinateSolver->frameToPointPro_JOINT(frameIn, pointOut,isSinguratyOut,configString,
                                 positionInitialList,isErrorNotice))
    {
        return -1;
    }
    return 1;
}

int CoordinateManager::isPointFutCorrect(PointPro checkPoint, vector<double> initialJointPosition)
{
    std::string tmpConfigString;
    std::vector<int> tmpConfigInt;
    if(-1==coordinateSolver->getMultiConfig(initialJointPosition, tmpConfigString, tmpConfigInt))
    {
          qDebug()<<"error1,,,isPointFutCorrect";
          return -1;
    }

    if(checkPoint.configString!=tmpConfigString && !checkPoint.configString.empty())
    {
        if(0)
        {
            qDebug()<<"error2,,,isPointFutCorrect checkPoint.configString"<<QString::fromStdString(checkPoint.configString)
                   <<"tmpConfigString"<<QString::fromStdString(tmpConfigString);
        }

        return -1;
    }
    else
    {
        return 1;
    }
}


QDataStream &operator <<(QDataStream &out, PointPro &pointPro)
{
    //qt5.6 todo
//    if(1==pointPro.jointFlag)//笛卡尔坐标不存储
//    {
//        return out;
//    }
//    out << (quint32)pointPro.pointNum;
////    qDebug() << __LINE__<<"===pointPro.pointNum=="<<pointPro.pointNum;
//    out << (quint32)pointPro.groupNum;
////    qDebug() << __LINE__<<"===pointPro.groupNum=="<<pointPro.groupNum;
//    quint32 tmp;
//    QVector<float> joints;
//    tmp = pointPro.positionValue.size();
//    joints = QVector<float>::fromStdVector(pointPro.positionValue);
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp==POSITIONSIZE=="<<tmp;
//    for(quint32 i = 0; i < tmp;++i)
//    {
//        out << joints[i];
////        qDebug() << __LINE__<<"=== joints[i]=="<< joints[i];
//    }
//    return out;
}

QDataStream &operator >>(QDataStream &in, PointPro &pointPro)
{
    //qt5.6 todo
//    pointPro.jointFlag = JOINT_FLAG_JOINT;
//    pointPro.configString.clear();
//    pointPro.configInt.clear();
//    pointPro.extPositionValue.clear();
//    quint32 tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    pointPro.pointNum = tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    pointPro.groupNum = tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    QVector<float> joints;
//    joints.resize(tmp);
//    for (quint32 i = 0; i < tmp;++i)
//    {
//        in >> joints[i];
////        qDebug() << __LINE__<<"===joints[i]===="<<joints[i];
//    }
//    pointPro.positionValue = joints.toStdVector();
//    return in;
}

QDataStream& operator <<(QDataStream &out, const Frame &frame)
{
    //qt5.6 todo
//    QVector<double> tmp;
//    tmp.resize(12);
//    for(int i = 0; i < 9;++i)
//    {
//        tmp[i] = frame.M.data[i];
//    }
//    for(int i = 0; i < 3;++i)
//    {
//        tmp[i+9] = frame.p[i];
//    }
//    for(quint32 i = 0; i < 12;++i)
//    {
//        out << tmp[i];
////        qDebug() << __LINE__<<"===tmp[i]===="<<tmp[i];
//    }
//    return out;
}

QDataStream& operator >>(QDataStream &in, Frame &frame)
{
    //qt5.6 todo
//    QVector<double> tmp;
//    tmp.resize(12);
//    for(quint32 i = 0; i < 12;++i)
//    {
//        in >> tmp[i];
////        qDebug() << __LINE__<<"===tmp[i]===="<<tmp[i];
//    }
//    for(int i = 0; i < 9;++i)
//    {
//        frame.M.data[i] = tmp[i];
//    }
//    for(int i = 0; i < 3;++i)
//    {
//        frame.p[i] = tmp[i+9];
//    }
//    return in;
}

QDataStream &operator <<(QDataStream &out, ToolCoordinateInfo_frame &toolCoordinate)
{
    //qt5.6 todo
//    quint32 tmp;
//    tmp =(quint32)toolCoordinate.index;
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    float error = toolCoordinate.error;
//    out << error;
////    qDebug() << __LINE__<<"===error===="<<error;
//    tmp = (quint32)toolCoordinate.method;
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    tmp = toolCoordinate.pointList.size();
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    for(quint32 i=0; i < tmp; i++)
//    {
//        out << toolCoordinate.pointList[i];
////        qDebug() << __LINE__<<"===tmp===="<<tmp;
//    }
//    for(quint32 i=0; i <6; i++)
//    {
//        out << toolCoordinate.xyzwpr[i];
////        qDebug() << __LINE__<<"===toolCoordinate.xyzwpr[i]===="<<toolCoordinate.xyzwpr[i];
//    }
//    out << toolCoordinate.forwardTcpMatrix;
//    out << toolCoordinate.inverseTcpMatrix;
//    return out;
}

QDataStream &operator >>(QDataStream &in, ToolCoordinateInfo_frame &toolCoordinate)
{
    //qt5.6 todo
//    quint32 tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    toolCoordinate.index = tmp;
//    float error;
//    in >> error;
////    qDebug() << __LINE__<<"===error===="<<error;
//    toolCoordinate.error =error;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    toolCoordinate.method = tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    toolCoordinate.pointList.resize(tmp);
//    for(quint32 i=0; i <tmp; i++)
//    {
//        in >> toolCoordinate.pointList[i];
////        qDebug() << __LINE__<<"===toolCoordinate.pointList[i]===="<<toolCoordinate.pointList[i];
//    }
//    for(quint32 i=0; i <6; i++)
//    {
//        in >> toolCoordinate.xyzwpr[i];
////        qDebug() << __LINE__<<"===toolCoordinate.xyzwpr[i]===="<<toolCoordinate.xyzwpr[i];
//    }
//    in >> toolCoordinate.forwardTcpMatrix;
//    in >> toolCoordinate.inverseTcpMatrix;
//    return in;
}

QDataStream &operator <<(QDataStream &out, UserCoordinateInfo_frame &userCoordinate)
{
    //qt5.6 todo
//    quint32 tmp;
//    tmp = (quint32)userCoordinate.index;
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    tmp = (quint32)userCoordinate.method;
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    tmp = userCoordinate.pointList.size();
//    out << tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    for(quint32 i = 0; i < tmp;++i)
//    {
//        out << userCoordinate.pointList[i];
////        qDebug() << __LINE__<<"===userCoordinate.pointList[i]===="<<userCoordinate.pointList[i];
//    }
//    for(quint32 i = 0; i < 6;++i)
//    {
//        out << userCoordinate.xyzwpr[i];
////        qDebug() << __LINE__<<"===userCoordinate.xyzwpr[i]===="<<userCoordinate.xyzwpr[i];
//    }
//    out << userCoordinate.forwardTcpMatrix;
//    out << userCoordinate.inverseTcpMatrix;
//    return out;
}

QDataStream &operator >>(QDataStream &in, UserCoordinateInfo_frame &userCoordinate)
{
    //qt5.6 todo
//    quint32 tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    userCoordinate.index = tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    userCoordinate.method = tmp;
//    in >> tmp;
////    qDebug() << __LINE__<<"===tmp===="<<tmp;
//    userCoordinate.pointList.resize(tmp);
//    for(quint32 i = 0; i < tmp;++i)
//    {
//        in >> userCoordinate.pointList[i];
////        qDebug() << __LINE__<<"===userCoordinate.pointList[i]===="<<userCoordinate.pointList[i];
//    }
//    userCoordinate.xyzwpr.resize(6);
//    for(quint32 i = 0; i < 6;++i)
//    {
//        in >> userCoordinate.xyzwpr[i];
////        qDebug() << __LINE__<<"===userCoordinate.xyzwpr[i]===="<<userCoordinate.xyzwpr[i];
//    }
//    in >> userCoordinate.forwardTcpMatrix;
//    in >> userCoordinate.inverseTcpMatrix;
    return in;
}

int CoordinateManager::readUserCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    if(1==readCoordinateFile_json(fileName,coordinateListOut))
    {
        usingMutex.lock();
        userCoordinateList.clear();
        UserCoordinateInfo_frame tmpCoordinate;
        for(int i=0;i<coordinateListOut.size();i++)
        {
            tmpCoordinate.index=coordinateListOut[i].index;
            tmpCoordinate.method=coordinateListOut[i].method;
            tmpCoordinate.name=coordinateListOut[i].name;
            tmpCoordinate.pointList=coordinateListOut[i].pointList;
            tmpCoordinate.xyzwpr=coordinateListOut[i].xyzwpr;
            tmpCoordinate.forwardTcpMatrix=coordinateListOut[i].forwardTcpMatrix;
            tmpCoordinate.inverseTcpMatrix=coordinateListOut[i].inverseTcpMatrix;

            userCoordinateList.append(tmpCoordinate);
        }
        usingMutex.unlock();
        return 1;
    }
    return -1;
    //qt5.6 todo
//    QFile file(fileName);
//    if(file.exists())
//    {
//        if(file.open(QIODevice::ReadOnly))
//        {
//            QDataStream in(&file);
//            in.setVersion(QDataStream::Qt_4_6);
//            quint32 tmpSize;
//            in >> tmpSize;

////            qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
////            userCoordinateList.resize(tmpSize);

//            if(tmpSize>COORDINATE_MAX_SIZE)
//            {
//                tmpSize = COORDINATE_MAX_SIZE;
//            }
//            for(quint32 i=1; i< tmpSize;i++)
//            {
//                in >> userCoordinateList[i];
//            }
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            //MD５文件校验
//             MD5Check md5Checker;
//             if(-1==md5Checker.isMd5CheckOk(fileName,fileName+".md5"))
//             {
//                 qDebug()<<"CoordinateManager::readToolCoordinateList error！-1==md5Checker.isMd5CheckOk";
//                 addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5054, robotId);
//                 return -1;
//             }
//            return 1;
//        }
//    }
//    else
//    {
//        //创建文件
//        if(file.open(QIODevice::ReadOnly))
//        {
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            return 0;
//        }
//    }
    return -1;
}

int CoordinateManager::writeUserCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    usingMutex.lock();
    CommonCoordinateInfo_frame tmpCoordinate;
    for(int i=0;i<userCoordinateList.size();i++)
    {
        tmpCoordinate.index=userCoordinateList[i].index;
        tmpCoordinate.error=0;
        tmpCoordinate.name=userCoordinateList[i].name;
        tmpCoordinate.method=userCoordinateList[i].method;
        tmpCoordinate.pointList=userCoordinateList[i].pointList;
        tmpCoordinate.xyzwpr=userCoordinateList[i].xyzwpr;
        tmpCoordinate.forwardTcpMatrix=userCoordinateList[i].forwardTcpMatrix;
        tmpCoordinate.inverseTcpMatrix=userCoordinateList[i].inverseTcpMatrix;

        coordinateListOut.append(tmpCoordinate);
    }
    usingMutex.unlock();
    if(1==writeCoordinateFile_json(fileName,coordinateListOut))
    {

        return 1;
    }
    return -1;
    //qt5.6 todo
//    QFile file(fileName);
//    if(!file.exists())
//    {
//        //创建文件
//        if(file.open(QIODevice::ReadWrite))
//        {
//            QDataStream out(&file);
//            out.setVersion(QDataStream::Qt_4_6);
//            quint32 tmpSize = COORDINATE_MAX_SIZE;
//            out << tmpSize;
////            qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
//            for(quint32 i=1; i<tmpSize;i++)
//            {
//                out << userCoordinateList[i];
//            }
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            //创建md5文件
//            MD5Check md5Checker;
//            md5Checker.createMd5File(fileName);
//            return 0;
//        }
//    }
//    if(file.open(QIODevice::WriteOnly))
//    {
//        QDataStream out(&file);
//        out.setVersion(QDataStream::Qt_4_6);
//        quint32 tmpSize = COORDINATE_MAX_SIZE;
//        out << tmpSize;
////        qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
//        for(quint32 i=1; i<tmpSize;i++)
//        {
//            out << userCoordinateList[i];
//        }
//        file.close();
//        file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//        //创建md5文件
//        MD5Check md5Checker;
//        md5Checker.createMd5File(fileName);
//        return 1;
//    }
//    return -1;
}

int CoordinateManager::readToolCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    if(1==readCoordinateFile_json(fileName,coordinateListOut))
    {
        usingMutex.lock();
        toolCoordinateList.clear();
        ToolCoordinateInfo_frame tmpCoordinate;
        for(int i=0;i<coordinateListOut.size();i++)
        {
            tmpCoordinate.name=coordinateListOut[i].name;
            tmpCoordinate.index=coordinateListOut[i].index;
            tmpCoordinate.error=coordinateListOut[i].error;
            tmpCoordinate.method=coordinateListOut[i].method;
            tmpCoordinate.pointList=coordinateListOut[i].pointList;
            tmpCoordinate.xyzwpr=coordinateListOut[i].xyzwpr;
            tmpCoordinate.forwardTcpMatrix=coordinateListOut[i].forwardTcpMatrix;
            tmpCoordinate.inverseTcpMatrix=coordinateListOut[i].inverseTcpMatrix;
            tmpCoordinate.mass=coordinateListOut[i].mass;
            tmpCoordinate.massCenter=coordinateListOut[i].massCenter;

            toolCoordinateList.append(tmpCoordinate);
        }
        usingMutex.unlock();
        return 1;
    }
    return -1;
    //qt5.6 todo
//    QFile file(fileName);
//    if(file.exists())
//    {
//            if(file.open(QIODevice::ReadOnly))
//            {
//                    QDataStream in(&file);
//                    in.setVersion(QDataStream::Qt_4_6);
//                    quint32 tmpSize;
//                    in >> tmpSize;
//        //            qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
//        //            toolCoordinateList.resize(tmpSize);

//        //            qDebug() << "***tmpSize***"<<tmpSize<<"\n\n";
//                    if(tmpSize>COORDINATE_MAX_SIZE)
//                    {
//                        tmpSize = COORDINATE_MAX_SIZE;
//                    }
//                    for(quint32 i=1; i<tmpSize;i++)
//                    {
//                        in >> toolCoordinateList[i];
//                    }
//                    file.close();
//                    file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//                   //MD５文件校验
//                    MD5Check md5Checker;
//                    if(-1==md5Checker.isMd5CheckOk(fileName,fileName+".md5"))
//                    {
//                        qDebug()<<"CoordinateManager::readToolCoordinateList error！-1==md5Checker.isMd5CheckOk";
//                        addMsg(ENUM_MSG_ERROR, "McControllerInterpreter", "CoordinateManager", 5053, robotId);
//                        return -1;
//                    }
//              }
//              return 1;
//    }
//    else
//    {
//        //创建文件
//        if(file.open(QIODevice::ReadOnly))
//        {
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            return 0;
//        }
//    }
    return -1;
}

int CoordinateManager::writeToolCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    usingMutex.lock();
    CommonCoordinateInfo_frame tmpCoordinate;
    for(int i=0;i<toolCoordinateList.size();i++)
    {
        tmpCoordinate.index=toolCoordinateList[i].index;
        tmpCoordinate.error=toolCoordinateList[i].error;
        tmpCoordinate.method=toolCoordinateList[i].method;
        tmpCoordinate.name=toolCoordinateList[i].name;
        tmpCoordinate.pointList=toolCoordinateList[i].pointList;
        tmpCoordinate.xyzwpr=toolCoordinateList[i].xyzwpr;
        tmpCoordinate.forwardTcpMatrix=toolCoordinateList[i].forwardTcpMatrix;
        tmpCoordinate.inverseTcpMatrix=toolCoordinateList[i].inverseTcpMatrix;
        tmpCoordinate.mass=toolCoordinateList[i].mass;
        tmpCoordinate.massCenter=toolCoordinateList[i].massCenter;

        coordinateListOut.append(tmpCoordinate);
    }
    usingMutex.unlock();
    if(1==writeCoordinateFile_json(fileName,coordinateListOut))
    {

        return 1;
    }
    return -1;
    //qt5.6 todo
//    QFile file(fileName);
//    if(!file.exists())
//    {
//        if(file.open(QIODevice::ReadWrite))//创建文件
//        {
//            QDataStream out(&file);
//            out.setVersion(QDataStream::Qt_4_6);
//            quint32 tmpSize = COORDINATE_MAX_SIZE;
//            out << tmpSize;
////            qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
//            for(quint32 i=1; i<tmpSize;i++)
//            {
//                out << toolCoordinateList[i];
//            }
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            //创建md5文件
//            MD5Check md5Checker;
//            md5Checker.createMd5File(fileName);
//            return 0;
//        }
//    }
//    else
//    {
//        if(file.open(QIODevice::WriteOnly))
//        {
//            QDataStream out(&file);
//            out.setVersion(QDataStream::Qt_4_6);
//            quint32 tmpSize = COORDINATE_MAX_SIZE;
//            out << tmpSize;
////            qDebug() <<__LINE__ <<"====tmpSize=="<<tmpSize;
//            for(quint32 i=1; i<tmpSize;i++)
//            {
//                out << toolCoordinateList[i];
//            }
//            file.close();
//            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
//            //创建md5文件
//            MD5Check md5Checker;
//            md5Checker.createMd5File(fileName);
//            return 1;
//        }
//    }
    return -1;
}

int CoordinateManager::readCameraCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    if(1==readCoordinateFile_json(fileName,coordinateListOut))
    {
        usingMutex.lock();
        cameratoolCoordinateList.clear();
        ToolCoordinateInfo_frame tmpCoordinate;
        for(int i=0;i<coordinateListOut.size();i++)
        {
            tmpCoordinate.name=coordinateListOut[i].name;
            tmpCoordinate.index=coordinateListOut[i].index;
            tmpCoordinate.error=coordinateListOut[i].error;
            tmpCoordinate.method=coordinateListOut[i].method;
            tmpCoordinate.pointList=coordinateListOut[i].pointList;
            tmpCoordinate.xyzwpr=coordinateListOut[i].xyzwpr;
            tmpCoordinate.forwardTcpMatrix=coordinateListOut[i].forwardTcpMatrix;
            tmpCoordinate.inverseTcpMatrix=coordinateListOut[i].inverseTcpMatrix;

            cameratoolCoordinateList.append(tmpCoordinate);
        }
        usingMutex.unlock();
        return 1;
    }
    return -1;
}

int CoordinateManager::writeCameraCoordinateList(QString fileName)
{
    QVector<CommonCoordinateInfo_frame> coordinateListOut;
    usingMutex.lock();
    CommonCoordinateInfo_frame tmpCoordinate;
    for(int i=0;i<cameratoolCoordinateList.size();i++)
    {
        tmpCoordinate.name=cameratoolCoordinateList[i].name;
        tmpCoordinate.index=cameratoolCoordinateList[i].index;
        tmpCoordinate.error=cameratoolCoordinateList[i].error;
        tmpCoordinate.method=cameratoolCoordinateList[i].method;
        tmpCoordinate.pointList=cameratoolCoordinateList[i].pointList;
        tmpCoordinate.xyzwpr=cameratoolCoordinateList[i].xyzwpr;
        tmpCoordinate.forwardTcpMatrix=cameratoolCoordinateList[i].forwardTcpMatrix;
        tmpCoordinate.inverseTcpMatrix=cameratoolCoordinateList[i].inverseTcpMatrix;

        coordinateListOut.append(tmpCoordinate);
    }
    usingMutex.unlock();
    if(1==writeCoordinateFile_json(fileName,coordinateListOut))
    {

        return 1;
    }
    return -1;

}

int CoordinateManager::readCoordinateFile_json(QString fileName,QVector<CommonCoordinateInfo_frame> &coordinateListOut)
{
    qDebug()<<"CoordinateManager::readCoordinateFile_json"<<fileName;
    coordinateListOut.clear();
    int returnFlag=-1;
    QFile file(fileName);
    if(file.exists())
    {
        if(file.open(QIODevice::ReadOnly))
        {

            QByteArray receiveByteArry = file.read(file.bytesAvailable());
            QJsonParseError jsonError;
            QJsonDocument receiveJsonDoc = QJsonDocument::fromJson(receiveByteArry, &jsonError);
            if(jsonError.error != QJsonParseError::NoError )
            {
              qDebug() << "readCoordinateFile_json JsonFile error "<<jsonError.error;

              return -2;
            }
            QJsonObject redeiveJsonObj = receiveJsonDoc.object();

            if(false==redeiveJsonObj.contains("regR_info"))
            {

                return -3;
            }
            QJsonObject obj_level1=redeiveJsonObj.value("regR_info").toObject();
            if(false==obj_level1.contains("size"))
            {

                return -4;
            }
            int size=obj_level1.value("size").toInt();
            if(false==obj_level1.contains("RValue_list"))
            {

                return -5;
            }
            QJsonArray arry_itemList=obj_level1.value("RValue_list").toArray();

            if(arry_itemList.size()!=size)
            {

                return -6;
            }
            QString tmpNodeName;
            for(int k=0;k<size;k++)
            {
                CommonCoordinateInfo_frame tmp_value;
                QJsonObject obj_item=arry_itemList[k].toObject();
                tmpNodeName="Coordinate"+QString::number(k+1);
                if(false==obj_item.contains(tmpNodeName))
                {

                    return -100-k;
                }
                QJsonObject obj_item_point=obj_item[tmpNodeName].toObject();

                if(1!=getCoordinateFromJson(obj_item_point,tmp_value))
                {

                    return -1000-k;
                }


                coordinateListOut.append(tmp_value);
            }


            file.close();
            file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
        }
        returnFlag= 1;
    }
    else
    {
//        regPrValue.resize(REG_SIZE);
        returnFlag= -1;
    }



    return returnFlag;
}

int CoordinateManager::writeCoordinateFile_json(QString fileNameIn,
                                                const QVector<CommonCoordinateInfo_frame> &coordinateListIn)
{
    //COORDINATE_MAX_SIZE
    qDebug() << "CoordinateManager::writeCoordinateFile_json";


    qDebug() << "===fileNameIn===="<<fileNameIn<<endl;

    QFile file(fileNameIn);
    if(!file.open(QIODevice::WriteOnly| QIODevice::Text))
    {
        //消息预留
        //保存R寄存器数据到文件失败
        return -1;
    }
    else
    {

        QJsonObject obj_root;
        QJsonObject obj_level1;
        QJsonArray arry_itemList;
        obj_level1.insert("size", coordinateListIn.size());

        QString tmpNodeName;
        for(int i=0;i<coordinateListIn.size();i++)
        {
            QJsonObject obj_item;
            tmpNodeName="Coordinate"+QString::number(i+1);
            QJsonObject obj_item_point;
            transCoordinateToJson(coordinateListIn[i],obj_item_point);
            obj_item.insert(tmpNodeName, obj_item_point);
            arry_itemList.append(obj_item);
        }

        obj_level1.insert("RValue_list", arry_itemList);
        obj_root.insert("regR_info", obj_level1);

        QJsonDocument jsonDoc(obj_root);
        QString tmpString=jsonDoc.toJson();

        //写入内容,这里需要转码，否则报错。
    //    QByteArray str = tmpString.toUtf8();
         QByteArray str = tmpString.toLatin1();
        //写入QByteArray格式字符串
        file.write(str);

        file.close();
        file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。
    }



    MD5Check md5Checker;
    md5Checker.createMd5File(fileNameIn);
    return 1;
}

int CoordinateManager::getCoordinateFromJson(const QJsonObject &obj_coordinate,
                                             CommonCoordinateInfo_frame &coordinateOut)
{
    CommonCoordinateInfo_frame tmpCoordinate;

    //
    if(false==obj_coordinate.contains("index"))
    {

        return -1;
    }
    tmpCoordinate.index=obj_coordinate.value("index").toInt();

    if(false==obj_coordinate.contains("error"))
    {

        return -2;
    }
    tmpCoordinate.error=obj_coordinate.value("error").toDouble();

    if(false==obj_coordinate.contains("name"))
    {

        return -2;
    }
    tmpCoordinate.name=obj_coordinate.value("name").toString().toStdString();

    if(false==obj_coordinate.contains("method"))
    {

        return -3;
    }
    tmpCoordinate.method=obj_coordinate.value("method").toInt();

    if(false==obj_coordinate.contains("pointList"))
    {

        return -4;
    }
    QJsonArray obj_pointList=obj_coordinate.value("pointList").toArray();
    QJsonObject obj_point;
    for(int i=0;i<obj_pointList.size();i++)
    {
        PointPro tmpPoint;
        obj_point=obj_pointList[i].toObject();
        getPointProFromJson(obj_point,tmpPoint);
        tmpCoordinate.pointList.append(tmpPoint);

    }

    if(false==obj_coordinate.contains("xyzwpr"))
    {

        return -4;
    }
    QJsonArray obj_xyzwpr=obj_coordinate.value("xyzwpr").toArray();
    for(int i=0;i<obj_xyzwpr.size();i++)
    {
        tmpCoordinate.xyzwpr.append(obj_xyzwpr[i].toDouble());

    }

    if(false==obj_coordinate.contains("forwardTcpMatrix"))
    {

        return -3;
    }
    QJsonObject obj_forwardTcpMatrix=obj_coordinate.value("forwardTcpMatrix").toObject();
    if(1!=getFrameFromJson(obj_forwardTcpMatrix,tmpCoordinate.forwardTcpMatrix))
    {
        return -5;
    }

    if(false==obj_coordinate.contains("inverseTcpMatrix"))
    {

        return -3;
    }
    QJsonObject obj_inverseTcpMatrix=obj_coordinate.value("inverseTcpMatrix").toObject();
    if(1!=getFrameFromJson(obj_inverseTcpMatrix,tmpCoordinate.inverseTcpMatrix))
    {
        return -6;
    }

    if(false==obj_coordinate.contains("mass"))
    {

        return -7;
    }
    tmpCoordinate.mass=obj_coordinate.value("mass").toDouble();

    if(false==obj_coordinate.contains("center"))
    {

        return -8;
    }
    QJsonArray obj_center=obj_coordinate.value("center").toArray();
    for(int i=0;i<obj_center.size();i++)
    {
        tmpCoordinate.massCenter.append(obj_center[i].toDouble());

    }


    coordinateOut=tmpCoordinate;
    return 1;
}

int CoordinateManager::transCoordinateToJson(const CommonCoordinateInfo_frame &coordinateIn, QJsonObject &obj_out)
{
    QJsonObject obj_coordinate;
    obj_coordinate.insert("index", coordinateIn.index);
    obj_coordinate.insert("error", coordinateIn.error);
    obj_coordinate.insert("method", coordinateIn.method);
    obj_coordinate.insert("name", QString::fromStdString(coordinateIn.name));


    QJsonArray  obj_point_list;
    QJsonObject obj_point;
    for(int i=0;i<coordinateIn.pointList.size();i++)
    {
        transPointToJson(coordinateIn.pointList[i],obj_point);
        obj_point_list.append(QJsonValue(obj_point));
    }
    obj_coordinate.insert("pointList", obj_point_list);

    QJsonArray  obj_xyzwpr_list;
    for(int i=0;i<coordinateIn.xyzwpr.size();i++)
    {
        obj_xyzwpr_list.append(QJsonValue(coordinateIn.xyzwpr[i]));
    }
    obj_coordinate.insert("xyzwpr", obj_xyzwpr_list);

    QJsonObject obj_forwardTcpMatrix;
    transFrameToJson(coordinateIn.forwardTcpMatrix,obj_forwardTcpMatrix);
    obj_coordinate.insert("forwardTcpMatrix", obj_forwardTcpMatrix);

    QJsonObject obj_inverseTcpMatrix;
    transFrameToJson(coordinateIn.inverseTcpMatrix,obj_inverseTcpMatrix);
    obj_coordinate.insert("inverseTcpMatrix", obj_inverseTcpMatrix);

    obj_coordinate.insert("mass", coordinateIn.mass);

    QJsonArray  obj_center_list;
    for(int i=0;i<coordinateIn.massCenter.size();i++)
    {
        obj_center_list.append(QJsonValue(coordinateIn.massCenter[i]));
    }
    obj_coordinate.insert("center", obj_center_list);

    obj_out=obj_coordinate;
    return 1;
}


int CoordinateManager::getPointProFromJson(const QJsonObject &obj_item_point, PointPro &pointOut)
{
    PointPro tmpPoint;
    if(false==obj_item_point.contains("jointFlag"))
    {

        return -1;
    }
    tmpPoint.jointFlag=obj_item_point.value("jointFlag").toInt();
    if(false==obj_item_point.contains("pointNum"))
    {

        return -2;
    }
    tmpPoint.pointNum=obj_item_point.value("pointNum").toInt();
    if(false==obj_item_point.contains("groupNum"))
    {

        return -3;
    }
    tmpPoint.groupNum=obj_item_point.value("groupNum").toInt();
    if(false==obj_item_point.contains("ufValue"))
    {

        return -4;
    }
    tmpPoint.ufValue=obj_item_point.value("ufValue").toInt();
    if(false==obj_item_point.contains("utValue"))
    {

        return -5;
    }
    tmpPoint.utValue=obj_item_point.value("utValue").toInt();
    if(false==obj_item_point.contains("recordStatus"))
    {

        return -6;
    }
    tmpPoint.recordStatus=obj_item_point.value("recordStatus").toInt();
    if(false==obj_item_point.contains("configString"))
    {

        return -7;
    }
    tmpPoint.configString=obj_item_point.value("configString").toString().toStdString();
    //
    if(false==obj_item_point.contains("configInt"))
    {

        return -8;
    }
    QJsonArray tmp_arry=obj_item_point.value("configInt").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.configInt.push_back(tmp_arry[i].toInt());
    }
    //
    if(false==obj_item_point.contains("positionValue"))
    {

        return -9;
    }
    tmp_arry=obj_item_point.value("positionValue").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.positionValue.push_back(tmp_arry[i].toDouble());
    }

    //
    if(false==obj_item_point.contains("extPositionValue"))
    {

        return -10;
    }
    tmp_arry=obj_item_point.value("extPositionValue").toArray();
    for(int i=0;i<tmp_arry.size();i++)
    {
        tmpPoint.extPositionValue.push_back(tmp_arry[i].toDouble());
    }
    pointOut=tmpPoint;
    return 1;

}

int CoordinateManager::transPointToJson(const PointPro &pointIn, QJsonObject &obj_out)
{
    QJsonObject obj_item_point;
    obj_item_point.insert("jointFlag", pointIn.jointFlag);
    obj_item_point.insert("pointNum", pointIn.pointNum);
    obj_item_point.insert("groupNum", pointIn.groupNum);
    obj_item_point.insert("ufValue", pointIn.ufValue);
    obj_item_point.insert("utValue", pointIn.utValue);
    obj_item_point.insert("recordStatus", pointIn.recordStatus);
    obj_item_point.insert("configString", QString::fromStdString(pointIn.configString));


    QJsonArray arry_configInt,arry_positionValue,arry_extPositionValue;

    for(int i=0;i<pointIn.configInt.size();i++)
    {
        arry_configInt.append(QJsonValue(pointIn.configInt[i]));
    }
    obj_item_point.insert("configInt", arry_configInt);

    for(int i=0;i<pointIn.positionValue.size();i++)
    {
        arry_positionValue.append(QJsonValue(pointIn.positionValue[i]));
    }
    obj_item_point.insert("positionValue", arry_positionValue);

    for(int i=0;i<pointIn.extPositionValue.size();i++)
    {
        arry_extPositionValue.append(QJsonValue(pointIn.extPositionValue[i]));
    }
    obj_item_point.insert("extPositionValue", arry_extPositionValue);

    obj_out=obj_item_point;
    return 1;

}

int CoordinateManager::getFrameFromJson(const QJsonObject &obj_frame, Frame &frameOut)
{
    Frame tmpFrame;

    //
    if(false==obj_frame.contains("frame_p"))
    {

        return -1;
    }
    QJsonArray tmp_arry=obj_frame.value("frame_p").toArray();
    if(3!=tmp_arry.size())
    {
        return -3;
    }
    for(int i=0;i<3;i++)
    {
        tmpFrame.p.data[i]=tmp_arry[i].toDouble();
    }
    //
    if(false==obj_frame.contains("frame_M"))
    {

        return -2;
    }
    tmp_arry=obj_frame.value("frame_M").toArray();
    if(9!=tmp_arry.size())
    {
        return -3;
    }

    for(int i=0;i<9;i++)
    {
        tmpFrame.M.data[i]=tmp_arry[i].toDouble();
    }


    frameOut=tmpFrame;
    return 1;

}

int CoordinateManager::transFrameToJson(const Frame &frameIn, QJsonObject &obj_out)
{
    QJsonObject obj_frame;

    QJsonArray arry_p,arry_M;

    for(int i=0;i<3;i++)
    {
        arry_p.append(QJsonValue(frameIn.p.data[i]));
    }
    obj_frame.insert("frame_p", arry_p);

    for(int i=0;i<9;i++)
    {
        arry_M.append(QJsonValue(frameIn.M.data[i]));
    }
    obj_frame.insert("frame_M", arry_M);


    obj_out=obj_frame;
    return 1;
}


void CoordinateManager::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotIdIn,
                                     int parameter1, int parameter2, int parameter3, int parameter4, QString strms)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageCode = messageCode;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotIdIn;

    string infomationStr;

    switch(messageCode)
    {
    case 5069:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")20点标定已完成!").toStdString()+strms.toStdString();
        infomationStr = str1;
        break;
    }
    case 5068:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")正在进行20点标定，请等待完成提示!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5067:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")工具(%1号)重量写入成功!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5066://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")相机坐标系文件读取失败，请联系供应商！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5065:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")相机坐标系(%1号)计算失败!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5064:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")相机坐标系(%1号)写入成功!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }

    case 5063://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")保存相机坐标系文件失败！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5062:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")工具坐标系(%1号)计算失败!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5061:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")用户坐标系(%1号)计算失败!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5060:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")工具坐标系(%1号)写入成功!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5059:
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")用户坐标系(%1号)写入成功!").arg(parameter1).toStdString();
        infomationStr = str1;
        break;
    }
    case 5058://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")保存工具坐标系文件失败！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5057://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")保存用户坐标系文件失败！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5056://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")工具坐标系文件读取失败，请联系供应商！").toStdString();
        infomationStr = str1;
        break;
    }
    case 5055://ENUM_MSG_ERROR
    {
        string str1;
        str1 = QObject::tr("机器人(ID:").toStdString() +
                QString::number(robotIdIn).toStdString() +
                QObject::tr(")用户坐标系文件读取失败，请联系供应商！").toStdString();
        infomationStr = str1;
        break;
    }
            case 5054://ENUM_MSG_ERROR
            {
                string str1;
                str1 = QObject::tr("机器人(ID:").toStdString() +
                        QString::number(robotIdIn).toStdString() +
                        QObject::tr(")用户坐标系文件校验出错，请联系供应商！").toStdString();
                infomationStr = str1;
                break;
            }

            case 5053://ENUM_MSG_ERROR
            {
                string str1;
                str1 = QObject::tr("机器人(ID:").toStdString() +
                        QString::number(robotIdIn).toStdString() +
                        QObject::tr(")工具坐标系文件校验出错，请联系供应商！").toStdString();
                infomationStr = str1;
                break;
            }
            case 5052://ENUM_MSG_ERROR
            {
                string str1;
                str1 = QObject::tr("机器人(ID:").toStdString() +
                        QString::number(robotIdIn).toStdString() +
                        QObject::tr(")点位信息里面存储的工具坐标号(%1)与但前工具坐标号(%2)不一致!").arg(parameter1)
                        .arg(parameter2).toStdString();
                infomationStr = str1;
                break;
            }
            case 5051://ENUM_MSG_ERROR
            {
                string str1;
                str1 = QObject::tr("机器人(ID:").toStdString() +
                        QString::number(robotIdIn).toStdString() +
                        QObject::tr(")点位信息里面存储的用户坐标号(%1)与但前用户坐标号(%2)不一致!").arg(parameter1)
                        .arg(parameter2).toStdString();
                infomationStr = str1;
                break;
            }
        case 5050://ENUM_MSG_ERROR
        {
            string str1;
            str1 = QObject::tr("机器人(ID:").toStdString() +
                    QString::number(robotIdIn).toStdString() +
                    QObject::tr(")迪卡尔点转关节点失败！").toStdString();
            infomationStr = str1;
            break;
        }
        default:
        {
            qDebug()<<"CoordinateSolver::addMsg====not match error code";
            break;
        }
    }

    tmpMsg.MessageInformation = infomationStr;

    #ifndef MOTION_SERVER_UNIT_TEST
    motionMessage->addMsg(tmpMsg);
    #endif
}
